/*! 
 * Implementation file for the contact resolution system.
 *
 * \author Devis Rossini
 * \date 19 March 2012
 */
#include "PhxContacts.h"

#include <memory>
#include <cassert>

using namespace phx;

void Contact::SetBodyData(RigidBody* one, RigidBody* two, real friction, real restitution)
{
	mBodies[0]		= one;
	mBodies[1]		= two;

	assert(mBodies[0] || mBodies[1]);

	mFriction		= friction;
	mRestitution	= restitution;
}

void Contact::CalculateInternals(real duration)
{
	// Check if the first object is NULL, and swap if it is
	if (!mBodies[0]) SwapBodies();
	assert(mBodies[0]);

	// Calculate a set of axis at the contact point
	CalculateContactBasis();

	// Store the relative position of the contact relative to each body
	mRelativeContactPosition[0] = mContactPoint - mBodies[0]->GetPosition();
	if (mBodies[1]) 
	{
		mRelativeContactPosition[1] = mContactPoint - mBodies[1]->GetPosition();
	}

	// Find the relative velocity of the bodies at the contact point
	mContactVelocity = CalculateLocalVelocity(0, duration);
	if (mBodies[1]) 
	{
		mContactVelocity -= CalculateLocalVelocity(1, duration);
	}

	// Calculate the desired change in velocity for resolution
	CalculateDesiredDeltaVelocity(duration);
}

void Contact::SwapBodies()
{
    mContactNormal *= -1;

    RigidBody* temp = mBodies[0];
    mBodies[0]		= mBodies[1];
    mBodies[1]		= temp;
}

void Contact::MatchAwakeState()
{
	// Collisions with the world never cause a body to wake up
	if (!mBodies[1]) return;

	bool firstBodyAwake		= mBodies[0]->GetAwake();
	bool secondBodyAwake	= mBodies[1]->GetAwake();

	// Wake up only the sleeping one
	if (firstBodyAwake ^ secondBodyAwake) 
	{
		if (firstBodyAwake) mBodies[1]->SetAwake();
		else mBodies[0]->SetAwake();
	}
}

void Contact::CalculateDesiredDeltaVelocity(real duration)
{
	const static real velocityLimit = (real)0.25;

	// Calculate the acceleration induced velocity accumulated this frame
	real velocityFromAcc = 0;

	if (mBodies[0]->GetAwake())
	{
		mBodies[0]->GetLastFrameAcceleration() * duration * mContactNormal;
	}

	if (mBodies[1] && mBodies[1]->GetAwake())
	{
		velocityFromAcc -=
			mBodies[1]->GetLastFrameAcceleration() * duration * mContactNormal;
	}

	// If the velocity is very slow, limit the restitution
	real thisRestitution = mRestitution;
	if (real_abs(mContactVelocity.x) < velocityLimit)
	{
		thisRestitution = (real)0.0f;
	}

	// Combine the bounce velocity with the removed acceleration velocity
	mDesiredDeltaVelocity =
		- mContactVelocity.x
		- thisRestitution * (mContactVelocity.x - velocityFromAcc);
}

Vector3 Contact::CalculateLocalVelocity(unsigned bodyIndex, real duration)
{
    RigidBody* thisBody = mBodies[bodyIndex];

    // Work out the velocity of the contact point
    Vector3 velocity =
        thisBody->GetAngularVelocity() % mRelativeContactPosition[bodyIndex];
    velocity += thisBody->GetVelocity();

    // Turn the velocity into contact space
    Vector3 contactVelocity = mContactToWorld.TransformTranspose(velocity);

    // Calculate the ammount of velocity that is due to forces without
    // reactions
    Vector3 accVelocity = thisBody->GetLastFrameAcceleration() * duration;

    // Calculate the velocity in contact space
    accVelocity = mContactToWorld.TransformTranspose(accVelocity);

    // We ignore any component of acceleration in the contact normal
    // direction, we are only interested in planar acceleration
    accVelocity.x = 0;

    // Add the planar velocities - if there's enough friction they will
    // be removed during velocity resolution
    contactVelocity += accVelocity;

    return contactVelocity;
}

inline 
void Contact::CalculateContactBasis()
{
	Vector3 contactTangent[2];

	// Check whether the z-axis is nearer to the x or y axis
	if (real_abs(mContactNormal.x) > real_abs(mContactNormal.y))
	{
		// Scaling factor to ensure the results are normalised
		const real s = (real)1.0f / real_sqrt(mContactNormal.z * mContactNormal.z +
			mContactNormal.x * mContactNormal.x);

		// The new x-axis is at right angles to the world y-axis
		contactTangent[0].x = mContactNormal.z * s;
		contactTangent[0].y = 0;
		contactTangent[0].z = -mContactNormal.x * s;

		// The new y-axis is at right angles to the new z- and z- axes
		contactTangent[1].x = mContactNormal.y * contactTangent[0].x;
		contactTangent[1].y = mContactNormal.z * contactTangent[0].x -
			mContactNormal.x * contactTangent[0].z;
		contactTangent[1].z = -mContactNormal.y * contactTangent[0].x;
	}
	else
	{
		// Scaling factor to ensure the results are normalised
		const real s = (real)1.0 / real_sqrt(mContactNormal.z * mContactNormal.z +
			mContactNormal.y * mContactNormal.y);

		// The new x-axis is at right angles to the world x-axis
		contactTangent[0].x = 0;
		contactTangent[0].y = -mContactNormal.z * s;
		contactTangent[0].z = mContactNormal.y * s;

		// The new y-axis is at right angles to the new x- and z- axes
		contactTangent[1].x = mContactNormal.y * contactTangent[0].z -
			mContactNormal.z * contactTangent[0].y;
		contactTangent[1].y = -mContactNormal.x * contactTangent[0].z;
		contactTangent[1].z = mContactNormal.x * contactTangent[0].y;
	}

	// Make a matrix from the three vectors
	mContactToWorld.SetComponents(
		mContactNormal,
		contactTangent[0],
		contactTangent[1]);
}

void Contact::ApplyVelocityChange(Vector3 velocityChange[2], Vector3 angularVelocityChange[2])
{
    // Get hold of the inverse mass and inverse inertia tensor, both in
    // world coordinates
    Matrix3 inverseInertiaTensor[2];
    
	mBodies[0]->GetInverseInertiaTensorWorld(&inverseInertiaTensor[0]);
    
	if (mBodies[1])
	{
        mBodies[1]->GetInverseInertiaTensorWorld(&inverseInertiaTensor[1]);
	}

    // We will calculate the impulse for each contact axis
    Vector3 impulseContact;

    if (mFriction == (real)0.0)
    {
        // Use the short format for frictionless contacts
        impulseContact = CalculateFrictionlessImpulse(inverseInertiaTensor);
    }
    else
    {
        // Otherwise we may have impulses that aren't in the direction of the
        // contact, so we need the more complex version
        impulseContact = CalculateFrictionImpulse(inverseInertiaTensor);
    }

    // Convert impulse to world coordinates
    Vector3 impulse = mContactToWorld.Transform(impulseContact);

    // Split in the impulse into linear and rotational components
    Vector3 impulsiveTorque = mRelativeContactPosition[0] % impulse;

    angularVelocityChange[0] = inverseInertiaTensor[0].Transform(impulsiveTorque);

    velocityChange[0].Clear();
    velocityChange[0].AddScaledVector(impulse, mBodies[0]->GetInverseMass());

    // Apply the changes
    mBodies[0]->AddVelocity(velocityChange[0]);
    mBodies[0]->AddAngularVelocity(angularVelocityChange[0]);

    if (mBodies[1])
    {
        // Work out body one's linear and angular changes
        Vector3 impulsiveTorque = impulse % mRelativeContactPosition[1];

        angularVelocityChange[1] = inverseInertiaTensor[1].Transform(impulsiveTorque);

        velocityChange[1].Clear();
        velocityChange[1].AddScaledVector(impulse, -mBodies[1]->GetInverseMass());

        // And apply them
        mBodies[1]->AddVelocity(velocityChange[1]);
        mBodies[1]->AddAngularVelocity(angularVelocityChange[1]);
    }
}

void Contact::ApplyPositionChange(Vector3 linearChange[2], Vector3 angularChange[2], real penetration)
{
	const real angularLimit = (real)0.2;
	real angularMove[2];
	real linearMove[2];

	real totalInertia = 0;
	real linearInertia[2];
	real angularInertia[2];

	// We need to work out the inertia of each object in the direction
	// of the contact normal, due to angular inertia only
	for (unsigned i = 0; i < 2; ++i) if (mBodies[i])
	{
		Matrix3 inverseInertiaTensor;
		mBodies[i]->GetInverseInertiaTensorWorld(&inverseInertiaTensor);

		// Use the same procedure as for calculating frictionless
		// velocity change to work out the angular inertia
		Vector3 angularInertiaWorld =
			mRelativeContactPosition[i] % mContactNormal;

		angularInertiaWorld =
			inverseInertiaTensor.Transform(angularInertiaWorld);

		angularInertiaWorld =
			angularInertiaWorld % mRelativeContactPosition[i];

		angularInertia[i] =
			angularInertiaWorld * mContactNormal;

		// The linear component is simply the inverse mass
		linearInertia[i] = mBodies[i]->GetInverseMass();

		// Keep track of the total inertia from all components
		totalInertia += linearInertia[i] + angularInertia[i];
	}

	// Loop through again calculating and applying the changes
	for (unsigned i = 0; i < 2; ++i) if (mBodies[i])
	{
		// The linear and angular movements required are in proportion to
		// the two inverse inertias
		real sign = (real)((i == 0) ? 1 : -1);

		angularMove[i] =
			sign * mPenetration * (angularInertia[i] / totalInertia);

		linearMove[i] =
			sign * mPenetration * (linearInertia[i] / totalInertia);

		// To avoid angular projections that are too great (when mass is large
		// but inertia tensor is small) limit the angular move
		Vector3 projection = mRelativeContactPosition[i];

		projection.AddScaledVector(
			mContactNormal,
			-mRelativeContactPosition[i].DotProduct(mContactNormal));

		// Use the small angle approximation for the sine of the angle
		real maxMagnitude = angularLimit * projection.Magnitude();

		if (angularMove[i] < -maxMagnitude)
		{
			real totalMove	= angularMove[i] + linearMove[i];
			angularMove[i]	= -maxMagnitude;
			linearMove[i]	= totalMove - angularMove[i];
		}
		else if (angularMove[i] > maxMagnitude)
		{
			real totalMove	= angularMove[i] + linearMove[i];
			angularMove[i]	= maxMagnitude;
			linearMove[i]	= totalMove - angularMove[i];
		}

		// We have the linear amount of movement required by turning
		// the rigid body in angularMove[i]. We now need to
		// calculate the desired rotation to achieve that
		if (angularMove[i] == 0)
		{
			// Easy case - no angular movement means no rotation
			angularChange[i].Clear();
		}
		else
		{
			// Work out the direction we'd like to rotate in
			Vector3 targetAngularDirection =
				mRelativeContactPosition[i].CrossProduct(mContactNormal);

			Matrix3 inverseInertiaTensor;
			mBodies[i]->GetInverseInertiaTensorWorld(&inverseInertiaTensor);

			// Work out the direction we'd need to rotate to achieve that
			angularChange[i] =
				inverseInertiaTensor.Transform(targetAngularDirection) *
				(angularMove[i] / angularInertia[i]);
		}

		// Velocity change is just the linear movement along the contact normal
		linearChange[i] = mContactNormal * linearMove[i];

		// Apply the linear movement
		Vector3 pos;
		mBodies[i]->GetPosition(&pos);
		pos.AddScaledVector(mContactNormal, linearMove[i]);
		mBodies[i]->SetPosition(pos);

		// Change the orientation
		Quaternion q;
		mBodies[i]->GetOrientation(&q);
		q.AddScaledVector(angularChange[i], ((real)1.0));
		mBodies[i]->SetOrientation(q);

		// We need to calculate the derived data for anymb body that is
		// asleep, so that the changes are reflected in the object's
		// data. Otherwise the resolution will not change the position
		// of the object, and the next collision detection round will
		// have the same penetration
		if (!mBodies[i]->GetAwake()) mBodies[i]->CalculateDerivedData();
	}
}

inline
Vector3 Contact::CalculateFrictionlessImpulse(Matrix3* inverseInertiaTensor)
{
    Vector3 impulseContact;

    // Build a vector that shows the change in velocity in
    // world space for a unit impulse in the direction of the contact
    // normal
    Vector3 deltaVelWorld = mRelativeContactPosition[0] % mContactNormal;
    deltaVelWorld = inverseInertiaTensor[0].Transform(deltaVelWorld);
    deltaVelWorld = deltaVelWorld % mRelativeContactPosition[0];

    // Work out the change in velocity in contact coordiantes
    real deltaVelocity = deltaVelWorld * mContactNormal;

    // Add the linear component of velocity change
    deltaVelocity += mBodies[0]->GetInverseMass();

    // Check if we need to the second body's data
    if (mBodies[1])
    {
        // Go through the same transformation sequence again
        Vector3 deltaVelWorld = mRelativeContactPosition[1] % mContactNormal;
        deltaVelWorld = inverseInertiaTensor[1].Transform(deltaVelWorld);
        deltaVelWorld = deltaVelWorld % mRelativeContactPosition[1];

        // Add the change in velocity due to rotation
        deltaVelocity += deltaVelWorld * mContactNormal;

        // Add the change in velocity due to linear motion
        deltaVelocity += mBodies[1]->GetInverseMass();
    }

    // Calculate the required size of the impulse
    impulseContact.x = mDesiredDeltaVelocity / deltaVelocity;
    impulseContact.y = 0;
    impulseContact.z = 0;

    return impulseContact;
}

inline
Vector3 Contact::CalculateFrictionImpulse(Matrix3* inverseInertiaTensor)
{
    Vector3 impulseContact;
    real inverseMass = mBodies[0]->GetInverseMass();

    // The equivalent of a cross product in matrices is multiplication
    // by a skew symmetric matrix - we build the matrix for converting
    // between linear and angular quantities
    Matrix3 impulseToTorque;
    impulseToTorque.SetSkewSymmetric(mRelativeContactPosition[0]);

    // Build the matrix to convert contact impulse to change in velocity
    // in world coordinates
    Matrix3 deltaVelWorld = impulseToTorque;
    deltaVelWorld *= inverseInertiaTensor[0];
    deltaVelWorld *= impulseToTorque;
    deltaVelWorld *= -1;

    // Check if we need to add body two's data
    if (mBodies[1])
    {
        // Set the cross product matrix
        impulseToTorque.SetSkewSymmetric(mRelativeContactPosition[1]);

        // Calculate the velocity change matrix
        Matrix3 deltaVelWorld2 = impulseToTorque;
        deltaVelWorld2 *= inverseInertiaTensor[1];
        deltaVelWorld2 *= impulseToTorque;
        deltaVelWorld2 *= -1;

        // Add to the total delta velocity
        deltaVelWorld += deltaVelWorld2;

        // Add to the inverse mass
        inverseMass += mBodies[1]->GetInverseMass();
    }

    // Do a change of basis to convert into contact coordinates
    Matrix3 deltaVelocity = mContactToWorld.Transpose();
    deltaVelocity *= deltaVelWorld;
    deltaVelocity *= mContactToWorld;

    // Add in the linear velocity change
    deltaVelocity.data[0] += inverseMass;
    deltaVelocity.data[4] += inverseMass;
    deltaVelocity.data[8] += inverseMass;

    // Invert to get the impulse needed per unit velocity
    Matrix3 impulseMatrix = deltaVelocity.Inverse();

    // Find the target velocities to kill
    Vector3 velKill(mDesiredDeltaVelocity,
        -mContactVelocity.y,
        -mContactVelocity.z);

    // Find the impulse to kill target velocities
    impulseContact = impulseMatrix.Transform(velKill);

    // Check for exceeding friction
    real planarImpulse = real_sqrt(
        impulseContact.y * impulseContact.y +
        impulseContact.z * impulseContact.z);

    if (planarImpulse > impulseContact.x * mFriction)
    {
        // We need to use dynamic friction
        impulseContact.y /= planarImpulse;
        impulseContact.z /= planarImpulse;

        impulseContact.x = deltaVelocity.data[0] +
            deltaVelocity.data[1] * mFriction * impulseContact.y +
            deltaVelocity.data[2] * mFriction * impulseContact.z;
        
		impulseContact.x = mDesiredDeltaVelocity / impulseContact.x;
        impulseContact.y *= mFriction * impulseContact.x;
        impulseContact.z *= mFriction * impulseContact.x;
    }

    return impulseContact;
}

ContactResolver::ContactResolver(unsigned iterations, real velocityEpsilon, real positionEpsilon)
{
	SetIterations(iterations);
	SetEpsilon(velocityEpsilon, positionEpsilon);
}

ContactResolver::ContactResolver(unsigned velocityIterations, unsigned positionIterations,
	real velocityEpsilon, real positionEpsilon)
{
	SetIterations(velocityIterations, positionIterations);
	SetEpsilon(velocityEpsilon, positionEpsilon);
}

void ContactResolver::SetIterations(unsigned iterations)
{
	SetIterations(iterations, iterations);
}

void ContactResolver::SetIterations(unsigned velocityIterations, unsigned positionIterations)
{
	mVelocityIterations = velocityIterations;
	mPositionIterations = positionIterations;
}

void ContactResolver::SetEpsilon(real velocityEpsilon, real positionEpsilon)
{
	mVelocityEpsilon = velocityEpsilon;
	mPositionEpsilon = positionEpsilon;
}

void ContactResolver::ResolveContacts(Contact* contacts, unsigned numContacts, real duration)
{
	// Make sure we have something to do
	if (numContacts == 0) return;
	if (!IsValid()) return;

	// Prepare the contacts for processing
	PrepareContacts(contacts, numContacts, duration);

	// Resolve the interpenetration problems with the contacts
	AdjustPositions(contacts, numContacts, duration);

	// Resolve the velocity problems with the contacts
	AdjustVelocities(contacts, numContacts, duration);
}

void ContactResolver::PrepareContacts(Contact* contacts, unsigned numContacts, real duration)
{
	// Generate contact velocity and axis information
	Contact* lastContact = contacts + numContacts;
	for (Contact* contact = contacts; contact < lastContact; ++contact)
	{
		// Calculate the internal contact data (inertia, basis, etc)
		contact->CalculateInternals(duration);
	}
}

void ContactResolver::AdjustVelocities(Contact* contacts, unsigned numContacts, real duration)
{
	Vector3 velocityChange[2], rotationChange[2];
	Vector3 deltaVel;

	// Iteratively handle impacts in order of severity
	mVelocityIterationsUsed = 0;
	while (mVelocityIterationsUsed < mVelocityIterations)
	{
		// Find contact with maximum magnitude of probable velocity change.
		real max = mVelocityEpsilon;
		unsigned index = numContacts;
		for (unsigned i = 0; i < numContacts; ++i)
		{
			if (contacts[i].mDesiredDeltaVelocity > max)
			{
				max = contacts[i].mDesiredDeltaVelocity;
				index = i;
			}
		}

		if (index == numContacts) break;

		// Match the awake state at the contact
		contacts[index].MatchAwakeState();

		// Do the resolution on the contact that came out top
		contacts[index].ApplyVelocityChange(velocityChange, rotationChange);

		// With the change in velocity of the two bodies, the update of
		// contact velocities means that some of the relative closing
		// velocities need recomputing
		for (unsigned i = 0; i < numContacts; ++i)
		{
			// Check each body in the contact
			for (unsigned b = 0; b < 2; ++b) if (contacts[i].mBodies[b])
			{
				// Check for a match with each body in the newly
				// resolved contact
				for (unsigned d = 0; d < 2; ++d)
				{
					if (contacts[i].mBodies[b] == contacts[index].mBodies[d])
					{
						deltaVel = velocityChange[d] +
							rotationChange[d].CrossProduct(contacts[i].mRelativeContactPosition[b]);

						// The sign of the change is negative if we're dealing
						// with the second body in a contact
						contacts[i].mContactVelocity += 
							contacts[i].mContactToWorld.TransformTranspose(deltaVel) * (real)(b ? -1 : 1);
						
						contacts[i].CalculateDesiredDeltaVelocity(duration);
					}
				}
			}
		}

		++mVelocityIterationsUsed;
	}
}

void ContactResolver::AdjustPositions(Contact* contacts, unsigned numContacts, real duration)
{
	unsigned i, index;
	Vector3 linearChange[2], angularChange[2];
	real max;
	Vector3 deltaPosition;

	// Iteratively resolve interpenetrations in order of severity
	mPositionIterationsUsed = 0;
	while (mPositionIterationsUsed < mPositionIterations)
	{
		// Find biggest penetration
		max = mPositionEpsilon;
		index = numContacts;
		for (i = 0; i < numContacts; ++i)
		{
			if (contacts[i].mPenetration > max)
			{
				max = contacts[i].mPenetration;
				index = i;
			}
		}

		if (index == numContacts) break;

		// Match the awake state at the contact
		contacts[index].MatchAwakeState();

		// Resolve the penetration
		contacts[index].ApplyPositionChange(
			linearChange,
			angularChange,
			max);

		// Again this action may have changed the penetration of other
		// bodies, so we update contacts
		for (i = 0; i < numContacts; ++i)
		{
			// Check each body in the contact
			for (unsigned b = 0; b < 2; ++b) if (contacts[i].mBodies[b])
			{
				// Check for a match with each body in the newly
				// resolved contact
				for (unsigned d = 0; d < 2; ++d)
				{
					if (contacts[i].mBodies[b] == contacts[index].mBodies[d])
					{
						deltaPosition = linearChange[d] +
							angularChange[d].CrossProduct(contacts[i].mRelativeContactPosition[b]);

						// The sign of the change is negative if we're dealing
						// with the second body in a contact
						contacts[i].mPenetration +=
							deltaPosition.DotProduct(contacts[i].mContactNormal) * (real)(b ? 1 : -1);
					}
				}
			}
		}

		++mPositionIterationsUsed;
	}
}
